package projects.ufam.filters;

import java.awt.Color;
import java.awt.GridLayout;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.geom.Point2D;
import java.util.Random;

import javax.swing.JFrame;
import javax.swing.JOptionPane;

import org.jfree.chart.ChartFactory;
import org.jfree.chart.ChartPanel;
import org.jfree.chart.JFreeChart;
import org.jfree.chart.plot.PlotOrientation;
import org.jfree.chart.renderer.xy.XYDotRenderer;
import org.jfree.chart.renderer.xy.XYLineAndShapeRenderer;
import org.jfree.data.xy.XYSeries;
import org.jfree.data.xy.XYSeriesCollection;

public class ParticleFilterDemo {

	private static final double STANDARD_DEVIATION = 1;
	private static final double MEAN = 0;
	private static final int NUMBER_PARTICLES = 1000;

	private double[] xr, yr, xm, ym, xe, ye;
	private ParticleFilter.Particle[][] particles;

	public ParticleFilterDemo(Trajectory trajectory) {
		Random randx = new Random();
		Random randy = new Random();

		xr = new double[trajectory.numberPoints()];
		yr = new double[trajectory.numberPoints()];
		xm = new double[trajectory.numberPoints()];
		ym = new double[trajectory.numberPoints()];
		xe = new double[trajectory.numberPoints()];
		ye = new double[trajectory.numberPoints()];
		particles = new ParticleFilter.Particle[trajectory.numberPoints()][NUMBER_PARTICLES];

		double emm = 0;
		double eme = 0;

		ParticleFilter particleFilter = new ParticleFilter(NUMBER_PARTICLES);

		for (int k = 0; k < trajectory.numberPoints(); k++) {

			// obtendo as partículas
			particles[k] = particleFilter.getParticles();

			// Estimação
			double estimated[] = null;
			if (k == 0)
				estimated = particleFilter.step(new double[] { 0, 0 });
			else
				estimated = particleFilter.step(new double[] { xm[k - 1],
						ym[k - 1] });
			xe[k] = estimated[0];
			ye[k] = estimated[1];

			// Deslocamento Real no tempo k.
			xr[k] = trajectory.pointAt(k).getX();
			yr[k] = trajectory.pointAt(k).getY();

			// Valor medido no tempo k
			xm[k] = xr[k] + randx.nextGaussian();
			ym[k] = yr[k] + randy.nextGaussian();

			// Erro da Estimação e da Medição no tempo k+1
			double em = Math.pow(Point2D.distance(xr[k], yr[k], xm[k], ym[k]),
					1);
			double ee = Math.pow(Point2D.distance(xr[k], yr[k], xe[k], ye[k]),
					1);

			// Acumulando o erro para calcular a media
			emm += em;
			eme += ee;
		}
		System.out.println("Erro Médio da Medição: "
				+ (emm / trajectory.numberPoints()));
		System.out.println("Erro Médio da Estimação: "
				+ (eme / trajectory.numberPoints()));
	}

	public Point2D getRealPoint(int step) {
		return new Point2D.Double(xr[step], yr[step]);
	}

	public Point2D getMeasuredPoint(int step) {
		return new Point2D.Double(xm[step], ym[step]);
	}

	public Point2D getEstimatedPoint(int step) {
		return new Point2D.Double(xe[step], ye[step]);
	}

	public ParticleFilter.Particle[] getParticles(int step) {
		return particles[step];
	}

	public int size() {
		return xr.length;
	}

	public static void main(String[] args) {
		Trajectory trajectory = new Trajectory();
		trajectory.generateTrajectory(new Point2D.Double(0, 0), 1000, 1); // trajectory.randomWalk(50);
		ParticleFilterDemo demo = new ParticleFilterDemo(trajectory);
		ParticleFilterDemoGraphDynamic g = new ParticleFilterDemoGraphDynamic(
				demo, true);
		g.setSize(550, 800);
		g.setLocationRelativeTo(null);
		g.setVisible(true);
		// g.setExtendedState(javax.swing.JFrame.MAXIMIZED_BOTH);
	}
}

class ParticleFilterDemoGraphDynamic extends JFrame implements KeyListener {

	private static final long serialVersionUID = 6475111446550844679L;
	private ParticleFilterDemo particleFilterDemo;
	private JFreeChart trajectoryChart, errorChart;
	private ChartPanel trajectoryPanel, errorPanel;
	private int step;
	private XYSeries realSeries, measuredSeries, estimatedSeries,
			particlesSeries;
	private XYSeries measuredErrorSeries, estimatedErrorSeries;

	public ParticleFilterDemoGraphDynamic(
			ParticleFilterDemo particleFilterDemo, boolean dynamic) {
		super("Gráfico - Pressione \"enter\" para visualizar cada passo!");
		this.step = 0;
		this.particleFilterDemo = particleFilterDemo;
		realSeries = new XYSeries("Real", false);
		measuredSeries = new XYSeries("Medido", false);
		estimatedSeries = new XYSeries("Estimado", false);
		particlesSeries = new XYSeries("Partículas", false);
		measuredErrorSeries = new XYSeries("Erro das medidas", false);
		estimatedErrorSeries = new XYSeries("Erro das Estimativas", false);

		XYSeriesCollection trajectoryDataset = new XYSeriesCollection();
		trajectoryDataset.addSeries(realSeries);
		trajectoryDataset.addSeries(estimatedSeries);
		trajectoryDataset.addSeries(measuredSeries);

		XYSeriesCollection particlesDataset = new XYSeriesCollection();
		particlesDataset.addSeries(particlesSeries);

		XYSeriesCollection errorDataset = new XYSeriesCollection();
		errorDataset.addSeries(estimatedErrorSeries);
		errorDataset.addSeries(measuredErrorSeries);

		if (!dynamic) {
			for (int x = 0; x < particleFilterDemo.size(); x++)
				nextStep();
			this.setTitle("Gráfico");
		} else
			this.addKeyListener(this);

		XYLineAndShapeRenderer renderer1 = new XYLineAndShapeRenderer();
		XYDotRenderer renderer2 = new XYDotRenderer();
		renderer2.setDotHeight(4);
		renderer2.setDotWidth(4);
		renderer2.setSeriesPaint(0, new Color(170, 156, 0));

		trajectoryChart = ChartFactory.createXYLineChart("", "X", "Y",
				trajectoryDataset, PlotOrientation.VERTICAL, true, true, false);
		trajectoryChart.setBackgroundPaint(java.awt.Color.white);
		trajectoryChart.getPlot().setBackgroundPaint(java.awt.Color.white);
		trajectoryChart.getXYPlot()
				.setDomainGridlinePaint(java.awt.Color.black);
		trajectoryChart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black);
		trajectoryChart.getXYPlot().setDataset(1, particlesDataset);
		trajectoryChart.getXYPlot().setRenderer(0, renderer1);
		trajectoryChart.getXYPlot().setRenderer(1, renderer2);

		errorChart = ChartFactory.createXYLineChart("", "Tempo", "Erro",
				errorDataset, PlotOrientation.VERTICAL, true, true, false);
		// errorChart.getXYPlot().setRenderer( new XYLineAndShapeRenderer() );
		errorChart.getXYPlot().getRenderer().setSeriesPaint(0,
				java.awt.Color.BLUE);
		errorChart.getXYPlot().getRenderer().setSeriesPaint(1,
				java.awt.Color.GREEN);
		errorChart.setBackgroundPaint(java.awt.Color.white);
		errorChart.getPlot().setBackgroundPaint(java.awt.Color.white);
		errorChart.getXYPlot().setDomainGridlinePaint(java.awt.Color.black);
		errorChart.getXYPlot().setRangeGridlinePaint(java.awt.Color.black);

		this.setLayout(new GridLayout(2, 1));
		this.trajectoryPanel = new ChartPanel(trajectoryChart);
		this.errorPanel = new ChartPanel(errorChart);
		this.getContentPane().add(trajectoryPanel);
		this.getContentPane().add(errorPanel);

	}

	public void nextStep() {
		if (step < particleFilterDemo.size()) {
			Point2D realPoint = particleFilterDemo.getRealPoint(step);
			Point2D measuredPoint = particleFilterDemo.getMeasuredPoint(step);
			Point2D estimatedPoint = particleFilterDemo.getEstimatedPoint(step);
			ParticleFilter.Particle[] particlesPoints = particleFilterDemo
					.getParticles(step);
			double measuredError = realPoint.distance(measuredPoint);
			double estimatedError = realPoint.distance(estimatedPoint);

			realSeries.add(realPoint.getX(), realPoint.getY());
			measuredSeries.add(measuredPoint.getX(), measuredPoint.getY());
			estimatedSeries.add(estimatedPoint.getX(), estimatedPoint.getY());
			measuredErrorSeries.add(step, measuredError);
			estimatedErrorSeries.add(step, estimatedError);

			particlesSeries.clear();
			for (int x = 0; x < particlesPoints.length; x++)
				particlesSeries.add(particlesPoints[x].getXPosition(),
						particlesPoints[x].getYPosition());
			this.setTitle("Gráfico - " + (step + 1) + "/"
					+ particleFilterDemo.size()
					+ " - Pressione L para recomeçar");
			step++;
		}
	}

	public void clear() {
		step = 0;
		realSeries.clear();
		measuredSeries.clear();
		estimatedSeries.clear();
		measuredErrorSeries.clear();
		particlesSeries.clear();
		estimatedErrorSeries.clear();
		this
				.setTitle("Gráfico - Pressione \"enter\" para visualizar cada passo!");
	}

	public void keyPressed(KeyEvent e) {
		if (e.getKeyChar() == 'l') {
			clear();
		} else if (e.getKeyChar() == KeyEvent.VK_ENTER) {
			nextStep();
			/*
			 * try{ rastroam.io.IO.exportEPSChart( this.trajectoryChart,
			 * this.trajectoryPanel.getWidth(),
			 * this.trajectoryPanel.getHeight(), "files/pf/trajectory" +
			 * this.step + ".eps" ); rastroam.io.IO.exportEPSChart(
			 * this.errorChart, this.errorPanel.getWidth(),
			 * this.errorPanel.getHeight(), "files/pf/error" + this.step +
			 * ".eps" ); ChartUtilities.saveChartAsJPEG(new
			 * File("files/pf/trajectory" + this.step + ".jpg"),
			 * this.trajectoryChart, this.trajectoryPanel.getWidth(),
			 * this.trajectoryPanel.getHeight());
			 * ChartUtilities.saveChartAsJPEG(new File("files/pf/error" +
			 * this.step + ".jpg"), this.errorChart, this.errorPanel.getWidth(),
			 * this.errorPanel.getHeight()); } catch( Exception ex ) {}
			 */
		} else if (e.getKeyCode() == KeyEvent.VK_P) {
			if (JOptionPane.showConfirmDialog(this, "Graph2Eps") == JOptionPane.YES_OPTION) {
				try {
					// rastroam.io.IO.exportEPSChart( this.trajectoryChart,
					// this.trajectoryPanel.getWidth(),
					// this.trajectoryPanel.getHeight(), "files/trajectory" +
					// this.step + ".eps" );
					// rastroam.io.IO.exportEPSChart( this.errorChart,
					// this.errorPanel.getWidth(), this.errorPanel.getHeight(),
					// "files/error" + this.step + ".eps" );
				} catch (Exception ex) {
				}
			}
		}

	}

	public void keyReleased(KeyEvent e) {
	}

	public void keyTyped(KeyEvent e) {
	}

}